# -*- coding: utf-8 -*-
import queue
import logging
import argparse
import time
import os
from rich.logging import RichHandler
from logging.handlers import RotatingFileHandler

from service.camera import Camera
from service.mcu_com import Mcu_com
from service.planner import Planner
from service.execute import Executor
from service.record import Record
# from service.udpIn import UdpIn
from service.udpOut import UdpOut
from service.pattern import Pattern
from service.line_patrol import LinePatrol


# CRITICAL = 50
# ERROR    = 40
# WARNING  = 30
# INFO     = 20
# DEBUG    = 10  # 调试信息，通常用于开发阶段，包含函数调用、变量值等
VERBOSE = 9  # 详细的调试信息，通常用于开发阶段，包含函数调用、变量值等
TRACE = 5  # 追踪程序执行的每一步，函数的每次进入和返回，复杂流程中的中间状态
SPAM = 3  # 极度频繁的、极低价值的调试信息，默认关闭，只有在需要深入追踪细节时才开启

START_TIME = time.strftime("%Y%m%d-%H%M", time.localtime())

class CustomLogger(logging.getLoggerClass()):
    def verbose(self, msg, *args, **kwargs):
        if self.isEnabledFor(VERBOSE):
            self._log(VERBOSE, msg, args, **kwargs)

    def trace(self, msg, *args, **kwargs):
        if self.isEnabledFor(TRACE):
            self._log(TRACE, msg, args, **kwargs)

    def spam(self, msg, *args, **kwargs):
        if self.isEnabledFor(SPAM):
            self._log(SPAM, msg, args, **kwargs)


class Service:
    def __init__(self):
        self.__parse_args()
        self.__log_init()
        # 任务队列创建
        # camera -> pattern         路口检测图像帧队列
        self.frame_pattern_queue = queue.Queue(maxsize=3)       # rgb frame: [frame, number]
        # camera -> planner         数字识别图像帧队列
        self.frame_detect_queue = queue.Queue(maxsize=2)        # rgb frame: [frame, number]
        # camera -> line_patrol     巡线图像帧队列
        self.frame_line_queue = queue.Queue(maxsize=2)          # rgb frame: [frame, number]

        # pattern -> planner        路口检测结果队列
        self.pattern_queue = queue.Queue(maxsize=1)             # pattern result: [signal, signal_ratio]    signal: [if_crossing, if_door], signal_ratio[crossing_ratio, door_ratio] 
        # line_patrol -> planner  巡线图像帧队列
        self.line_queue = queue.Queue(maxsize=1)                # line patrol result: [pid_out, number]

        # pattern -> recorder1      巡线结果队列
        self.recorder1_queue = queue.Queue(maxsize=1)           # recorder1: [rgb, number]

        # planner -> execute        任务信息队列      
        self.task_queue = queue.Queue(maxsize=3)                # task info: [task, number], task:[{"task":"task name"}]       
        
        # executer -> mcucom        下位机通信队列    
        self.speed_queue = queue.Queue(maxsize=1)               # speed control：[line_speed, angular_speed, number]
        self.info_queue = queue.Queue(maxsize=3)                # info to print：[line, (str)information, number]
        # mcucom -> planner         下位机通信队列
        self.object_queue = queue.Queue(maxsize=1)              # object alive: [if_alive]

        # udpin -> planner          UDP接收队列 
        self.udpin_queue = queue.Queue(maxsize=1)               # udp in: [data, number]

        # executor -> udpout        UDP发送队列
        self.udpout_queue = queue.Queue(maxsize=1)              # udp out: [data, number]


    # 任务初始化检查
    def init(self):
        self.logger.trace('init.enter')

        ret = self.__camera_init()
        if not ret:
            return False

        ret = self.__pattern_init()
        if not ret:
            return False

        ret = self.__line_patrol_init()
        if not ret:
            return False

        # ret = self.__udpin_init()
        # if not ret:
        #     return False

        ret = self.__planner_init()
        if not ret:
            return False

        ret = self.__executer_init()
        if not ret:
            return False

        ret = self.__mcu_com_init()
        if not ret:
            return False

        ret = self.__recorder1_init()
        if not ret:
            return False

        ret = self.__udpout_init()
        if not ret:
            return False

        self.logger.trace('init.done')
        return True

    # 任务关闭
    def deinit(self):
        self.logger.trace('deinit.enter')
        self.mcu_com.close()
        self.udp_out.close()
        self.executer.close()
        self.planner.close()
        self.recorder1.close()
        self.pattern.close()
        self.line_patrol.close()
        # self.udp_in.close()
        self.camera.close()
        self.logger.trace('deinit.done')

    # 任务启动
    def start(self):
        self.logger.trace('start.enter')
        self.camera.start()
        self.pattern.start()
        self.line_patrol.start()
        # self.udp_in.start()
        self.planner.start()
        self.executer.start()
        self.udp_out.start()
        self.recorder1.start()
        self.mcu_com.start()
        self.logger.trace('start.done')
        return True

    # 任务停止
    def stop(self):
        self.logger.trace('stop.enter')
        self.mcu_com.stop()
        self.udp_out.stop()
        self.executer.stop()
        self.planner.stop()
        self.recorder1.stop()
        self.pattern.stop()
        self.line_patrol.stop()
        # self.udp_in.stop()
        self.camera.stop()
        self.logger.trace('stop.done')


    def __parse_args(self):
        parser = argparse.ArgumentParser(description="Set log level.")

        parser.add_argument(
            "--log",
            choices=["spam", "trace", "verbose", "debug", "info", "warning", "error"],
            default="info",
            help="Choose the logger print level: spam, trace, verbose, debug or info",
        )

        args = parser.parse_args()

        if args.log == "spam":
            self.level = SPAM
        elif args.log == "trace":
            self.level = TRACE
        elif args.log == "verbose":
            self.level = VERBOSE
        elif args.log == "debug":
            self.level = logging.DEBUG
        elif args.log == "info":
            self.level = logging.INFO
        elif args.log == "warning":
            self.level = logging.WARNING
        elif args.log == "error":
            self.level = logging.ERROR
        else:
            self.level = logging.INFO

    def __log_init(self):
        logging.addLevelName(VERBOSE, "VERBOSE")
        logging.addLevelName(TRACE, "TRACE")
        logging.addLevelName(SPAM, "SPAM")
        logging.setLoggerClass(CustomLogger)

        formatter = logging.Formatter(
            "%(asctime)s %(levelname)-8s %(name)-8s %(message)s"
        )

        console_handler = RichHandler()
        console_handler.setLevel(level=self.level)
        console_handler.setFormatter(formatter)

        log_file = "logs/service.log"
        log_file_size = 8 * 1024 * 1024
        log_file_count = 128
        file_handler = RotatingFileHandler(
            log_file, maxBytes=log_file_size, backupCount=log_file_count
        )
        file_handler.setLevel(SPAM)
        file_handler.setFormatter(formatter)

        root_logger = logging.getLogger()
        root_logger.setLevel(SPAM)
        root_logger.addHandler(console_handler)
        root_logger.addHandler(file_handler)

        self.logger = logging.getLogger("Service")


    def __camera_init(self):
        self.logger.trace('__camera_init.enter')

        self.camera = Camera()
        ret = self.camera.open()
        if not ret:
            self.logger.error('camera open failed')
            return False

        camera_output_queue_list = [
            self.frame_detect_queue,
            self.frame_pattern_queue,
            self.frame_line_queue
        ]

        self.camera.output_queue_set(camera_output_queue_list)

        self.logger.trace('__camera_init.done')
        return True


    def __pattern_init(self):
        self.logger.trace('__pattern_init.enter')

        self.pattern = Pattern()

        binary_threshold = 40
        signal_detect_threshold = 0.25
        signal_detect_box = {
            "x0": 60, 
            "y0": 20, 
            "x1": 60+520, 
            "y1": 20+80
        }

        door_binary_threshold = 100
        door_signal_detect_threshold = 0.95
        door_signal_detect_box = {
            "x0": 0, 
            "y0": 0, 
            "x1": 640, 
            "y1": 480
        }

        ret = self.pattern.open(binary_threshold, 
                                signal_detect_threshold, 
                                signal_detect_box,
                                door_binary_threshold,
                                door_signal_detect_threshold,
                                door_signal_detect_box
                                )
        if not ret:
            self.logger.error('pattern open failed')
            return False

        pattern_input_queue_list = [
            self.frame_pattern_queue
        ]
        self.pattern.input_queue_set(pattern_input_queue_list)

        pattern_output_queue_list = [
            self.pattern_queue,
            self.recorder1_queue
        ]
        self.pattern.output_queue_set(pattern_output_queue_list)

        self.logger.trace('__pattern_init.done')
        return True


    def __line_patrol_init(self):
        self.logger.trace('__line_patrol_init.enter')

        self.line_patrol = LinePatrol()

        binary_threshold = 40
        search_line = 400   # 追踪第几行
        search_width = 10   # 宽度追踪左边起点

        ret = self.line_patrol.open(
            binary_threshold=binary_threshold,
            search_line=search_line,
            search_width=search_width
        )
        if not ret:
            self.logger.error('line patrol open failed')
            return False

        line_patrol_input_queue_list = [
            self.frame_line_queue
        ]
        self.line_patrol.input_queue_set(line_patrol_input_queue_list)

        line_patrol_output_queue_list = [
            self.line_queue
        ]
        self.line_patrol.output_queue_set(line_patrol_output_queue_list)

        self.logger.trace('__line_patrol_init.done')
        return True


    def __planner_init(self):
        self.logger.trace('__planner_init.enter')

        self.planner = Planner()

        model_path = 'algorithm/models/number_detection_v2.onnx'
        id_list = [
            '1','2','3','4','5','6','7','8'
        ]
        thred_cond = 0.80
        v_base = 0.30

        ret = self.planner.open(model_path, id_list, thred_cond, v_base)
        if not ret:
            self.logger.error('planner open failed')
            return False

        planner_input_queue_list = [
            self.frame_detect_queue,
            self.pattern_queue,
            self.line_queue,
            self.udpin_queue,
            self.object_queue
        ]

        self.planner.input_queue_set(planner_input_queue_list)

        planner_output_queue_list = [
            self.task_queue
        ]

        self.planner.output_queue_set(planner_output_queue_list)

        self.logger.trace('__planner_init.done')
        return True


    def __executer_init(self):
        self.logger.trace('__executer_init.enter')

        self.executer = Executor()

        ret = self.executer.open(
            0.30,
            {"Kp": 50,
             "Ki": 0,
             "Kd": 0,
             "setpoint": 0,
             "max_integral": 6,
             "min_integral": 6
             }
        )
        if not ret:
            self.logger.error('executer open failed')
            return False

        executer_input_queue_list = [
            self.task_queue
        ]

        self.executer.input_queue_set(executer_input_queue_list)

        executer_output_queue_list = [
            self.udpout_queue,
            self.speed_queue,
            self.info_queue
        ]

        self.executer.output_queue_set(executer_output_queue_list)

        self.logger.trace('__executer_init.done')
        return True


    def __recorder1_init(self):
        self.logger.trace('__recorder1_init.enter')

        self.recorder1 = Record()

        fps = 15
        width = 640
        height = 480
        recorder1_path = f'logs/record1/{START_TIME}.mp4'

        ret = self.recorder1.open(fps=fps, width=width, height=height, filename=recorder1_path)
        if not ret:
            self.logger.error('recorder1 open failed')
            return False

        recorder1_input_queue_list = [
            self.recorder1_queue
        ]

        self.recorder1.input_queue_set(recorder1_input_queue_list)

        self.logger.trace('__recorder1_init.done')
        return True


    def __mcu_com_init(self):
        self.logger.trace('__mcucom_init.enter')

        self.mcu_com = Mcu_com()

        serialDevice = "/dev/ttyS0"  # usart device ordinal

        self.mcu_com.open(serialDevice=serialDevice)

        self.mcu_com_input_queue_list = [
            self.speed_queue,  # speed control：[line_speed, angular_speed, number]
            self.info_queue,   # info to print：[line, (str)information, number]
        ]
        self.mcu_com.input_queue_set(self.mcu_com_input_queue_list)

        self.mcu_com_output_queue_list = [
            self.object_queue, # object alive: [if_alive]
        ]
        self.mcu_com.output_queue_set(self.mcu_com_output_queue_list)

        self.logger.trace('__mcucom_init.done')
        return True


    def __udpin_init(self):
        self.logger.trace('__udp_in_init.enter')

        self.udp_in = UdpIn()

        host = "0.0.0.0"
        port = 5010

        ret = self.udp_in.open(host, port)
        if not ret:
            self.logger.error('udp_in open failed')
            return False

        udpin_output_queue_list = [
            self.udpin_queue,  # udp in: [data, number]
        ]

        self.udp_in.output_queue_set(udpin_output_queue_list)

        self.logger.trace('__udp_in_init.done')
        return True


    def __udpout_init(self):
        self.logger.trace('__udp_out_init.enter')

        self.udp_out = UdpOut()

        host = "192.168.32.2"
        port = 5010

        ret = self.udp_out.open(host, port)
        if not ret:
            self.logger.error('udp_out open failed')
            return False

        udpout_input_queue_list = [
            self.udpout_queue,  # udp out: [data, number]
        ]

        self.udp_out.input_queue_set(udpout_input_queue_list)

        self.logger.trace('__udp_out_init.done')
        return True
